#include "../H/TankDrive.h"
#include "../H/Transmission.h"

//------------------------------------------------------------------------------
// METHOD:  TankDrive::TankDrive()
// Type:	Constructor
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
TankDrive::TankDrive()
{
	static const UINT8  DIGITAL_SIDECAR_MODULE   = 1;
	static const UINT32 RIGHT_REAR_MOTOR_CH      = 1;
	static const UINT32 RIGHT_FRONT_MOTOR_CH     = 2;
	static const UINT32 LEFT_REAR_MOTOR_CH       = 3;
	static const UINT32 LEFT_FRONT_MOTOR_CH      = 4;

	static const UINT32 LEFT_ENCODER_CHA         =  7;
	static const UINT32 LEFT_ENCODER_CHB         =  8;
	static const UINT32 RIGHT_ENCODER_CHA        =  9;
	static const UINT32 RIGHT_ENCODER_CHB        = 10;

	static const bool ENCODER_NORMAL_DIR         = true;
//	static const bool ENCODER_REVERSE_DIR        = false;

    static const double WHEEL_DIST_PER_PULSE     = 0.052;
	// Initialize wheel encoder variables
	// 19.5" per wheel rotation
	// 250 pulses per rotation of encoder
	// Gear ratio between encoder shaft and wheel shaft 24/36
	// = 19.50 / 250 * ( 24/36)

	pDriveRightRear  = new Talon(DIGITAL_SIDECAR_MODULE,RIGHT_REAR_MOTOR_CH);
	pDriveRightFront = new Talon(DIGITAL_SIDECAR_MODULE,RIGHT_FRONT_MOTOR_CH);
	pDriveLeftRear   = new Talon(DIGITAL_SIDECAR_MODULE,LEFT_REAR_MOTOR_CH);
	pDriveLeftFront  = new Talon(DIGITAL_SIDECAR_MODULE,LEFT_FRONT_MOTOR_CH);

	pRightEncoder    = new Encoder(RIGHT_ENCODER_CHA,RIGHT_ENCODER_CHB
			                      ,ENCODER_NORMAL_DIR);
	pLeftEncoder     = new Encoder(LEFT_ENCODER_CHA,LEFT_ENCODER_CHB
			                      ,ENCODER_NORMAL_DIR);
	pTransmission = new Transmission();

	pRightEncoder->SetDistancePerPulse(WHEEL_DIST_PER_PULSE);
	pLeftEncoder->SetDistancePerPulse(WHEEL_DIST_PER_PULSE);
}

TankDrive::TankDrive(UINT8 sideCarModule, UINT32 rRearMotorCh, UINT32 rFrontMotorCh
                                        , UINT32 lRearMotorCh, UINT32 lFrontMotorCh)
{
    static const UINT32 LEFT_ENCODER_CHA         =  7;
    static const UINT32 LEFT_ENCODER_CHB         =  8;
    static const UINT32 RIGHT_ENCODER_CHA        =  9;
    static const UINT32 RIGHT_ENCODER_CHB        = 10;

    static const bool ENCODER_NORMAL_DIR         = true;
//  static const bool ENCODER_REVERSE_DIR        = false;

    static const double WHEEL_DIST_PER_PULSE     = 0.052;
    // Initialize wheel encoder variables
    // 19.5" per wheel rotation
    // 250 pulses per rotation of encoder
    // Gear ratio between encoder shaft and wheel shaft 24/36
    // = 19.50 / 250 * ( 24/36)

    pDriveRightRear  = new Talon(sideCarModule,rRearMotorCh);
    pDriveRightFront = new Talon(sideCarModule,rFrontMotorCh);
    pDriveLeftRear   = new Talon(sideCarModule,lRearMotorCh);
    pDriveLeftFront  = new Talon(sideCarModule,lFrontMotorCh);

    pRightEncoder    = new Encoder(RIGHT_ENCODER_CHA,RIGHT_ENCODER_CHB
                                      ,ENCODER_NORMAL_DIR);
    pLeftEncoder     = new Encoder(LEFT_ENCODER_CHA,LEFT_ENCODER_CHB
                                      ,ENCODER_NORMAL_DIR);
	pTransmission = new Transmission();

    pRightEncoder->SetDistancePerPulse(WHEEL_DIST_PER_PULSE);
    pLeftEncoder->SetDistancePerPulse(WHEEL_DIST_PER_PULSE);
}

TankDrive::TankDrive(UINT8 sideCarModule, UINT32 rRearMotorCh, UINT32 rFrontMotorCh
                                        , UINT32 lRearMotorCh, UINT32 lFrontMotorCh
                                        , UINT32 lEncoderChA, UINT32 lEncoderChB
                                        , UINT32 rEncoderChA, UINT32 rEncoderChB)
{
    static const bool ENCODER_NORMAL_DIR         = true;
//  static const bool ENCODER_REVERSE_DIR        = false;

    pDriveRightRear  = new Talon(sideCarModule,rRearMotorCh);
    pDriveRightFront = new Talon(sideCarModule,rFrontMotorCh);
    pDriveLeftRear   = new Talon(sideCarModule,lRearMotorCh);
    pDriveLeftFront  = new Talon(sideCarModule,lFrontMotorCh);

    pRightEncoder    = new Encoder(rEncoderChA,rEncoderChB
                                      ,ENCODER_NORMAL_DIR);
    pLeftEncoder     = new Encoder(lEncoderChA,lEncoderChB
                                      ,ENCODER_NORMAL_DIR);
	pTransmission = new Transmission();
}
TankDrive::TankDrive(UINT32 rRearMotorCh, UINT32 rFrontMotorCh, UINT32 lRearMotorCh
                                        , UINT32 lFrontMotorCh, UINT32 lEncoderChA
                                        , UINT32 lEncoderChB, UINT32 rEncoderChA
                                        , UINT32 rEncoderChB)
{
    static const UINT8  DIGITAL_SIDECAR_MODULE   = 1;

    static const bool ENCODER_NORMAL_DIR         = true;
//  static const bool ENCODER_REVERSE_DIR        = false;

    static const double WHEEL_DIST_PER_PULSE     = 0.052;
    // Initialize wheel encoder variables
    // 19.5" per wheel rotation
    // 250 pulses per rotation of encoder
    // Gear ratio between encoder shaft and wheel shaft 24/36
    // = 19.50 / 250 * ( 24/36)

    pDriveRightRear  = new Talon(DIGITAL_SIDECAR_MODULE,rRearMotorCh);
    pDriveRightFront = new Talon(DIGITAL_SIDECAR_MODULE,rFrontMotorCh);
    pDriveLeftRear   = new Talon(DIGITAL_SIDECAR_MODULE,lRearMotorCh);
    pDriveLeftFront  = new Talon(DIGITAL_SIDECAR_MODULE,lFrontMotorCh);

    pRightEncoder    = new Encoder(rEncoderChA,rEncoderChB
                                      ,ENCODER_NORMAL_DIR);
    pLeftEncoder     = new Encoder(lEncoderChA,lEncoderChB
                                      ,ENCODER_NORMAL_DIR);
	pTransmission = new Transmission();

    pRightEncoder->SetDistancePerPulse(WHEEL_DIST_PER_PULSE);
    pLeftEncoder->SetDistancePerPulse(WHEEL_DIST_PER_PULSE);
}

TankDrive::TankDrive(UINT32 lEncoderChA, UINT32 lEncoderChB, UINT32 rEncoderChA
                                       , UINT32 rEncoderChB)
{
    static const UINT8  DIGITAL_SIDECAR_MODULE   = 1;
    static const UINT32 RIGHT_REAR_MOTOR_CH      = 1;
    static const UINT32 RIGHT_FRONT_MOTOR_CH     = 2;
    static const UINT32 LEFT_REAR_MOTOR_CH       = 3;
    static const UINT32 LEFT_FRONT_MOTOR_CH      = 4;

    static const bool ENCODER_NORMAL_DIR         = true;
//  static const bool ENCODER_REVERSE_DIR        = false;

    //static const double WHEEL_DIST_PER_PULSE     = 0.052;
    // Initialize wheel encoder variables
    // 19.5" per wheel rotation
    // 250 pulses per rotation of encoder
    // Gear ratio between encoder shaft and wheel shaft 24/36
    // = 19.50 / 250 * ( 24/36)

    pDriveRightRear  = new Talon(DIGITAL_SIDECAR_MODULE,RIGHT_REAR_MOTOR_CH);
    pDriveRightFront = new Talon(DIGITAL_SIDECAR_MODULE,RIGHT_FRONT_MOTOR_CH);
    pDriveLeftRear   = new Talon(DIGITAL_SIDECAR_MODULE,LEFT_REAR_MOTOR_CH);
    pDriveLeftFront  = new Talon(DIGITAL_SIDECAR_MODULE,LEFT_FRONT_MOTOR_CH);

    pRightEncoder    = new Encoder(rEncoderChA,rEncoderChB
                                      ,ENCODER_NORMAL_DIR);
    pLeftEncoder     = new Encoder(lEncoderChA,lEncoderChB
                                      ,ENCODER_NORMAL_DIR);
	pTransmission = new Transmission();
}
//------------------------------------------------------------------------------
// METHOD:  TankDrive::~TankDrive()
// Type:	Destructor
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
TankDrive::~TankDrive()
{
}
//------------------------------------------------------------------------------
// METHOD:  TankDrive::ResetWheelEncoders()
// Type:	Public Accessor for TankDrive
//------------------------------------------------------------------------------
// Resets the wheel encoders
//------------------------------------------------------------------------------
void TankDrive::ResetWheelEncoders()
{
	pRightEncoder->Stop();
	pRightEncoder->Reset();
	pRightEncoder->Start();

	pLeftEncoder->Stop();
	pLeftEncoder->Reset();
	pLeftEncoder->Start();

	return;
}
//------------------------------------------------------------------------------
// METHOD:  TankDrive::SetDriveMotors()
// Type:	Public Accessor for TankDrive
//------------------------------------------------------------------------------
// Resets the wheel encoders
//------------------------------------------------------------------------------
void TankDrive::SetDriveMotors(int tranSpeed, float leftJSThrottle
		                                    , float rightJSThrottle)
{
    static const float NORMAL_SPEED_ADJ = 1.00;
    static const float GRANNY_SPEED_ADJ = 0.30;

    float leftMotorSpeed;
    float rightMotorSpeed;

    if ( tranSpeed != pTransmission->GetGear())
    {
    	SmartDashboard::PutNumber("tranSpeed", tranSpeed);
    	tranSpeed = pTransmission->GetGear();
    }

    if ( tranSpeed == Transmission::kHighSpeed  ||
    	 tranSpeed == Transmission::kLowSpeed      )
    {
    	leftMotorSpeed  = NORMAL_SPEED_ADJ * leftJSThrottle;
    	rightMotorSpeed = NORMAL_SPEED_ADJ * rightJSThrottle;
    }
    else
    {
    	leftMotorSpeed  = GRANNY_SPEED_ADJ * leftJSThrottle;
    	rightMotorSpeed = GRANNY_SPEED_ADJ * rightJSThrottle;
    }

    pDriveLeftFront->Set(leftMotorSpeed);
    pDriveLeftRear->Set(leftMotorSpeed);
    pDriveRightFront->Set(rightMotorSpeed);
    pDriveRightRear->Set(rightMotorSpeed);

	return;
}
//------------------------------------------------------------------------------
// METHOD:  TankDrive::StopMotors()
// Type:	Public Accessor for TankDrive
//------------------------------------------------------------------------------
// Sets the wheel motor speed to zero to stop the robot
//------------------------------------------------------------------------------
void TankDrive::StopMotors()
{
    static const float STOP_MOTOR = 0.0;

    pDriveLeftFront->Set(STOP_MOTOR);
    pDriveLeftRear->Set(STOP_MOTOR);
    pDriveRightFront->Set(STOP_MOTOR);
    pDriveRightRear->Set(STOP_MOTOR);

	return;
}
//------------------------------------------------------------------------------
// METHOD:  TankDrive::GetMotorSpeed()
// Type:	Public Accessor for TankDrive
//------------------------------------------------------------------------------
// Gets the speed of the motors on the specified side
//------------------------------------------------------------------------------
float TankDrive::GetMotorSpeed(int wheelSide)
{
	float speed;

	if ( wheelSide == kLeftWheels )
		speed = pDriveLeftFront->Get();
	else
		speed = pDriveRightFront->Get();

	return speed;
}
//------------------------------------------------------------------------------
// METHOD:  TankDrive::GetDistance()
// Type:	Public Accessor for TankDrive
//------------------------------------------------------------------------------
// Sets the distance tracked by the wheel encoders
//------------------------------------------------------------------------------
double TankDrive::GetDistance(int wheelSide)
{
	double distance;

	if ( wheelSide == kLeftWheels )
		distance = pLeftEncoder->GetDistance();
	else
		distance = pRightEncoder->GetDistance();

	SmartDashboard::PutNumber("Left Encoder Pulses", pLeftEncoder->GetRaw());

	return distance;
}

//------------------------------------------------------------------------------
// METHOD:  TankDrive::ResetWheelEncoders()
// Type:	Public Accessor for TankDrive
//------------------------------------------------------------------------------
// Resets the wheel encoders
//------------------------------------------------------------------------------
void TankDrive::SetDistancePerPulse(float WHEEL_DIST_PER_PULSE)
{
	pRightEncoder->SetDistancePerPulse(WHEEL_DIST_PER_PULSE);
	pLeftEncoder->SetDistancePerPulse(WHEEL_DIST_PER_PULSE);

	return;
}
